'''
Created on Feb 25, 2019

@author: oelfarou
'''
from PID import PID as PID
from bicycle import  Bicycle as Bicycle
import math

if __name__ == '__main__':
############################################# Simulation Parameters ###############################################    
    __SimTime = 5
    __delta_t = 0.002
    __steps = int(__SimTime /__delta_t )
###################################################################################################################

############################################# Model Parameters ###################################################    
    __wheel_base = 1.5
    __phi = math.radians(10)
    __v = 10
    __max_steer = math.radians(35)
###################################################################################################################

############################################# Model Initialization #################################################     
    Model = Bicycle(__wheel_base,__max_steer,__steps,__delta_t)    
    
    x_init = 0
    y_init = 0
    theta_init = math.radians(0)
    
    Model.setPose(x_init,y_init,theta_init )
###################################################################################################################

############################################### Simulation Run ###################################################    
    HeadingCNTRL = PID(12,0.1,0.1,0.02)
   
    
    counter=[]
    
    theta_des = math.radians(20)
    theta_error = 0
    
    for i in range(0,__steps,1):
        
        [x_a ,y_a ,theta_a] = Model.getPose()
        
        theta_error = theta_des - theta_a
        
        phi = HeadingCNTRL.update(theta_error)
        
        Model.run(__v,phi, i)
        
        counter.append(i*__delta_t)

    Model.show(counter)
########################